Advanced Lane Finding Project¶

The goals / steps of this project are the following:

Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.
Apply a distortion correction to raw images.
Use color transforms, gradients, etc., to create a thresholded binary image.
Apply a perspective transform to rectify binary image ("birds-eye view").
Detect lane pixels and fit to find the lane boundary.
Determine the curvature of the lane and vehicle position with respect to center.
Warp the detected lane boundaries back onto the original image.
Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.

First, I'll compute the camera calibration using chessboard images¶

import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt
%matplotlib inline

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*9,3), np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.

# Make a list of calibration images
images = glob.glob('camera_cal/calibration*.jpg')
images_tbc = []

# Step through the list and search for chessboard corners
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    # Find the chessboard corners
    ret, corners = cv2.findChessboardCorners(gray, (9,6),None)

    # If found, add object points, image points
    if ret == True:
        objpoints.append(objp)
        imgpoints.append(corners)

        # Draw and display the corners
        img = cv2.drawChessboardCorners(img, (9,6), corners, ret)
        plt.figure()
        plt.imshow(img)
        plt.title(fname)
    else:
        print(fname+": corners not found")
        images_tbc.append(fname)

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, (img.shape[1],img.shape[0]), None, None)

camera_cal/calibration1.jpg: corners not found
camera_cal/calibration4.jpg: corners not found
camera_cal/calibration5.jpg: corners not found

Apply a distortion correction to raw images¶

for fname in images_tbc:
    # Read in an image
    img = cv2.imread(fname)
    undistorted = cv2.undistort(img, mtx, dist, None, mtx)
    plt.figure(figsize=(20,10))
    plt.subplot(1,2,1)
    plt.imshow(img)
    plt.title(fname)
    plt.subplot(1,2,2)
    plt.imshow(undistorted)
    plt.title(fname + " undistorted")

import matplotlib.image as mpimg

image = mpimg.imread('test_images/test5.jpg')
# Run the function
undist = cv2.undistort(image, mtx, dist, None, mtx)# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(image)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(undist)
ax2.set_title('Undistorted Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

Define gradient and color transforms to help detect lane lines¶

Sobel threshhold on x and y direction¶

# Define a function that applies Sobel x or y, 
# then takes an absolute value and applies a threshold.
# Note: calling your function with orient='x', thresh_min=5, thresh_max=100
# should produce output like the example image shown above this quiz.
def abs_sobel_thresh(img, orient='x', sobel_kernel = 3, thresh=(0,255)):
    
    # Apply the following steps to img
    # 1) Convert to grayscale
    # 2) Take the derivative in x or y given orient = 'x' or 'y'
    # 3) Take the absolute value of the derivative or gradient
    # 4) Scale to 8-bit (0 - 255) then convert to type = np.uint8
    # 5) Create a mask of 1's where the scaled gradient magnitude 
            # is > thresh_min and < thresh_max
    # 6) Return this mask as your binary_output image
    # Convert to HLS and pick the S channel
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
    s_channel = hls[:,:,2]
    # Apply x or y gradient with the OpenCV Sobel() function
    # and take the absolute value
    if orient == 'x':
        abs_sobel = np.absolute(cv2.Sobel(s_channel, cv2.CV_64F, 1, 0, ksize = sobel_kernel))
    if orient == 'y':
        abs_sobel = np.absolute(cv2.Sobel(s_channel, cv2.CV_64F, 0, 1, ksize = sobel_kernel))
    # Rescale back to 8 bit integer
    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
    # Create a copy and apply the threshold
    binary_output = np.zeros_like(scaled_sobel)
    # Here I'm using inclusive (>=, <=) thresholds, but exclusive is ok too
    binary_output[(scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1
    return binary_output

Sobel magnitube¶

# Define a function that applies Sobel x and y, 
# then computes the magnitude of the gradient
# and applies a threshold
def mag_thresh(img, sobel_kernel=3, mag_thresh=(0, 255)):
    
    # Apply the following steps to img
    # 1) Convert to grayscale
    # 2) Take the gradient in x and y separately
    # 3) Calculate the magnitude 
    # 4) Scale to 8-bit (0 - 255) and convert to type = np.uint8
    # 5) Create a binary mask where mag thresholds are met
    # 6) Return this mask as your binary_output image
    # Convert to HLS and pick the S channel
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
    s_channel = hls[:,:,2]
    # Take both Sobel x and y gradients
    sobelx = cv2.Sobel(s_channel, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobely = cv2.Sobel(s_channel, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    # Calculate the gradient magnitude
    gradmag = np.sqrt(sobelx**2 + sobely**2)
    # Rescale to 8 bit
    scale_factor = np.max(gradmag)/255 
    gradmag = (gradmag/scale_factor).astype(np.uint8) 
    # Create a binary image of ones where threshold is met, zeros otherwise
    binary_output = np.zeros_like(gradmag)
    binary_output[(gradmag >= mag_thresh[0]) & (gradmag <= mag_thresh[1])] = 1
    return binary_output

Direction of gradient after applying Sobel x and y¶

# Define a function that applies Sobel x and y, 
# then computes the direction of the gradient
# and applies a threshold.
def dir_threshold(img, sobel_kernel=3, thresh=(0, np.pi/2)):
    
    # Apply the following steps to img
    # 1) Convert to grayscale
    # 2) Take the gradient in x and y separately
    # 3) Take the absolute value of the x and y gradients
    # 4) Use np.arctan2(abs_sobely, abs_sobelx) to calculate the direction of the gradient 
    # 5) Create a binary mask where direction thresholds are met
    # 6) Return this mask as your binary_output image
    # Convert to HLS and pick the S channel
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
    s_channel = hls[:,:,2]
    # Calculate the x and y gradients
    sobelx = cv2.Sobel(s_channel, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobely = cv2.Sobel(s_channel, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    # Take the absolute value of the gradient direction, 
    # apply a threshold, and create a binary image result
    absgraddir = np.arctan2(np.absolute(sobely), np.absolute(sobelx))
    binary_output =  np.zeros_like(absgraddir)
    binary_output[(absgraddir >= thresh[0]) & (absgraddir <= thresh[1])] = 1
    return binary_output

Threshhold only¶

image = mpimg.imread('test_images/test5.jpg')
# Run the function
grad_binary = abs_sobel_thresh(image, orient='x', sobel_kernel=5, thresh=(20, 100))
# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(image)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(grad_binary, cmap='gray')
ax2.set_title('Thresholded Gradient', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

Threshhold Magnitude¶

# Run the function
mag_binary = mag_thresh(image, sobel_kernel=5, mag_thresh=(30, 150))
# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(image)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(mag_binary, cmap='gray')
ax2.set_title('Thresholded Magnitude', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

Threshold gradient with direction¶

# Run the function
dir_binary = dir_threshold(image, sobel_kernel=15, thresh=(0.9, 1.1))
# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(image)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(dir_binary, cmap='gray')
ax2.set_title('Thresholded Grad. Dir.', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

Combined Sobel gradient x and gradient y or magnitude and direction¶

# Choose a Sobel kernel size

# Apply each of the thresholding functions
gradx = abs_sobel_thresh(image, orient='x', sobel_kernel=5, thresh=(20, 100))
grady = abs_sobel_thresh(image, orient='y', sobel_kernel=5, thresh=(20, 100))
mag_binary = mag_thresh(image, sobel_kernel=5, mag_thresh=(30, 150))
dir_binary = dir_threshold(image, sobel_kernel=15, thresh=(0.9, 1.1))

sb_combined = np.zeros_like(dir_binary)
sb_combined[((gradx == 1) & (grady == 1)) | ((mag_binary == 1) & (dir_binary == 1))] = 1

# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(image)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(sb_combined, cmap='gray')
ax2.set_title('Thresholded Grad. Dir.', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

HLS color space with S and L channel threshold¶

# Define a function that thresholds the S-channel of HLS
# Use exclusive lower bound (>) and inclusive upper (<=)
def hls_select(img, s_thresh=(0, 255), l_thresh=(30,255)):
    # 1) Convert to HLS color space
    # 2) Apply a threshold to the S channel
    # 3) Return a binary image of threshold result
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
    s_channel = hls[:,:,2]
    l_channel = hls[:,:,1]
    binary_output = np.zeros_like(s_channel)
    binary_output[(s_channel > s_thresh[0]) & (s_channel <= s_thresh[1]) & (l_channel > l_thresh[0]) & (l_channel <= l_thresh[1])] = 1
    return binary_output

hls_binary = hls_select(image, s_thresh=(120, 255))

# Plot the result
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(image)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(hls_binary, cmap='gray')
ax2.set_title('Thresholded S and L', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

HLS color space threshold + Combined Sobel gradient threshold¶

# Stack each channel to view their individual contributions in green and blue respectively
# This returns a stack of the two binary images, whose components you can see as different colors
color_binary = np.dstack(( np.zeros_like(hls_binary), hls_binary, sb_combined))

# Combine the two binary thresholds
combined_binary = np.zeros_like(hls_binary)
combined_binary[(hls_binary == 1) | (sb_combined == 1)] = 1

# Plotting thresholded images
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(20,10))
ax1.set_title('Stacked thresholds')
ax1.imshow(color_binary)

ax2.set_title('Combined S and L channel and gradient thresholds')
ax2.imshow(combined_binary, cmap='gray')

<matplotlib.image.AxesImage at 0x14399c7f0>

Perspective transform¶

def corners_unwarp(img, src, mtx, dist):
    # Write code to do the following steps
    # 1) Undistort using mtx and dist
    # 2) define 4 destination points dst = np.float32([[,],[,],[,],[,]])
    # 3) use cv2.getPerspectiveTransform() to get M, the transform matrix
    # 4) use cv2.warpPerspective() to warp your image to a top-down view
    
    # Use the OpenCV undistort() function to remove distortion
    undist = cv2.undistort(img, mtx, dist, None, mtx)
    # Search for corners in the grayscaled image
    # Choose offset from image corners to plot detected corners
    # This should be chosen to present the result at the proper aspect ratio
    # My choice of 250 pixels is not exact, but close enough for our purpose here
    offset = 250 # offset for dst points
    # Grab the image shape
    img_size = (undist.shape[1], undist.shape[0])

    # For destination points, I'm arbitrarily choosing some points to be
    # a nice fit for displaying our warped result 
    # again, not exact, but close enough for our purposes
    dst = np.float32([[src[0,0] - offset, 0], [src[1,0] + offset, 0], \
                      [src[1,0] + offset, src[2,1]], [src[0,0] - offset, src[3,1]]])
    print(dst)
    # Given src and dst points, calculate the perspective transform matrix
    M = cv2.getPerspectiveTransform(src, dst)
    MInv = cv2.getPerspectiveTransform(dst, src)

    # Warp the image using OpenCV warpPerspective()
    warped = cv2.warpPerspective(undist, M, img_size, flags=cv2.INTER_LINEAR)

    return warped, M, MInv

image=plt.imread('test_images/straight_lines1.jpg')
src = np.float32([[592,450],[688,450],[1125,720],[190,720]])

cv2.line(image, tuple(src[0]), tuple(src[1]), color=[255,0,0], thickness=1)
cv2.line(image, tuple(src[1]), tuple(src[2]), color=[255,0,0], thickness=1)
cv2.line(image, tuple(src[2]), tuple(src[3]), color=[255,0,0], thickness=1)
cv2.line(image, tuple(src[3]), tuple(src[0]), color=[255,0,0], thickness=1)


top_down, M, MInv = corners_unwarp(image, src, mtx, dist)
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(image)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(top_down)
ax2.set_title('Undistorted and Warped Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

[[ 342.    0.]
 [ 938.    0.]
 [ 938.  720.]
 [ 342.  720.]]

Binarize the image and ploynomial fit the lane line¶

Binarize based on the settings in the previous section¶

def binarize(image):
    # Apply each of the thresholding functions
    gradx = abs_sobel_thresh(image, orient='x', sobel_kernel=5, thresh=(20, 100))
    grady = abs_sobel_thresh(image, orient='y', sobel_kernel=5, thresh=(20, 100))
    mag_binary = mag_thresh(image, sobel_kernel=5, mag_thresh=(30, 255))
    dir_binary = dir_threshold(image, sobel_kernel=15, thresh=(0.9, 1.1))

    sb_combined = np.zeros_like(dir_binary)
    sb_combined[((gradx == 1) & (grady == 1)) | ((mag_binary == 1) & (dir_binary == 1))] = 1
    
    hls_binary = hls_select(image, s_thresh=(120, 255))

    color_binary = np.dstack(( np.zeros_like(hls_binary), hls_binary, sb_combined))

    # Combine the two binary thresholds
    combined_binary = np.zeros_like(hls_binary)
    combined_binary[(hls_binary == 1) | (sb_combined == 1)] = 1
    
    return color_binary, combined_binary

Keep only the lane line part by masking the region of interest¶

def region_of_interest(img, vertices):
    """
    Applies an image mask.
    
    Only keeps the region of the image defined by the polygon
    formed from `vertices`. The rest of the image is set to black.
    """
    #defining a blank mask to start with
    mask = np.zeros_like(img)   
    
    #defining a 3 channel or 1 channel color to fill the mask with depending on the input image
    if len(img.shape) > 2:
        channel_count = img.shape[2]  # i.e. 3 or 4 depending on your image
        ignore_mask_color = (255,) * channel_count
    else:
        ignore_mask_color = 255
        
    #filling pixels inside the polygon defined by "vertices" with the fill color    
    cv2.fillPoly(mask, vertices, ignore_mask_color)
    
    #returning the image only where mask pixels are nonzero
    masked_image = cv2.bitwise_and(img, mask)
    return masked_image

Start developing a pipeline with previously defined methods: undistort + binarize¶

image=plt.imread('test_images/test1.jpg')
undist = cv2.undistort(image, mtx, dist, None, mtx)
plt.figure(figsize=(24,9))
plt.imshow(undist)
plt.title('Input image')
b_color, b_combined = binarize(undist)
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(b_color)
ax1.set_title('Stacked Color Gradient + Color space', fontsize=50)
ax2.imshow(b_combined, cmap='gray')
ax2.set_title('Binarized Combined', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

Region of interest + change perspective to bird eye view¶

imshape = b_combined.shape
vertices = np.array([[(100, imshape[0]),(imshape[1]/2-10, imshape[0]/2+42),(imshape[1]/2+10, imshape[0]/2+42), (imshape[1], imshape[0])]], dtype=np.int32)

roi = region_of_interest(b_combined, vertices)

img_size = (undist.shape[1], undist.shape[0])

# Warp the image using OpenCV warpPerspective()
binary_warped = cv2.warpPerspective(roi, M, img_size, flags=cv2.INTER_LINEAR)

f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(roi, cmap='gray')
ax1.set_title('Binary ROI Image', fontsize=50)
ax2.imshow(binary_warped, cmap='gray')
ax2.set_title('Binary Undistorted and Warped Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

Save the camera matrix, distortion coefficients, perspective matrix and its inverse for future use¶

import pickle
dist_pickle = {}
dist_pickle['mtx'] = mtx
dist_pickle['dist'] = dist
dist_pickle['M'] = M
dist_pickle['MInv'] = MInv
pickle.dump(dist_pickle, open( "dist.p", "wb" ) )

Use 2nd order polynominal to fit the lane line¶

def fit_lane_line(binary_warped, left_fit = None, right_fit = None):
    # Assuming you have created a warped binary image called "binary_warped"
    # Take a histogram of the bottom half of the image

    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = binary_warped.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255

    # only need to search within the margin of previous frame's curves
    if left_fit != None and right_fit != None:
        margin = 100
        left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + left_fit[2] + margin))) 
        right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + right_fit[2] + margin)))  
    else:
        print("first frame")
        histogram = np.sum(binary_warped[360:,:], axis=0)
        # Create an output image to draw on and  visualize the result
        # Find the peak of the left and right halves of the histogram
        # These will be the starting point for the left and right lines
        midpoint = np.int(histogram.shape[0]/2)
        leftx_base = np.argmax(histogram[:midpoint])
        rightx_base = np.argmax(histogram[midpoint:]) + midpoint

        # Choose the number of sliding windows
        nwindows = 9
        # Set height of windows
        window_height = np.int(binary_warped.shape[0]/nwindows)

        # Current positions to be updated for each window
        leftx_current = leftx_base
        rightx_current = rightx_base
        # Set the width of the windows +/- margin
        margin = 100
        # Set minimum number of pixels found to recenter window
        minpix = 50
        # Create empty lists to receive left and right lane pixel indices
        left_lane_inds = []
        right_lane_inds = []

        # Step through the windows one by one
        for window in range(nwindows):
            # Identify window boundaries in x and y (and right and left)
            win_y_low = binary_warped.shape[0] - (window+1)*window_height
            win_y_high = binary_warped.shape[0] - window*window_height
            win_xleft_low = leftx_current - margin
            win_xleft_high = leftx_current + margin
            win_xright_low = rightx_current - margin
            win_xright_high = rightx_current + margin
            # Draw the windows on the visualization image
            cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),(0,255,0), 2) 
            cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 2) 
            # Identify the nonzero pixels in x and y within the window
            good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]
            good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]
            # Append these indices to the lists
            left_lane_inds.append(good_left_inds)
            right_lane_inds.append(good_right_inds)
            # If you found > minpix pixels, recenter next window on their mean position
            if len(good_left_inds) > minpix:
                leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
            if len(good_right_inds) > minpix:        
                rightx_current = np.int(np.mean(nonzerox[good_right_inds]))

        # Concatenate the arrays of indices
        left_lane_inds = np.concatenate(left_lane_inds)
        right_lane_inds = np.concatenate(right_lane_inds)
    

    # Extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds] 
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds] 

    # Fit a second order polynomial to each
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    # Generate x and y values for plotting
    ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
    left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
    right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
    out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
    out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]

    return out_img, left_fitx, right_fitx, ploty, left_fit, right_fit

out_img, left_fitx, right_fitx, ploty, left_fit, right_fit = fit_lane_line(binary_warped)
plt.figure(figsize=(24, 9))
plt.imshow(out_img)
plt.plot(left_fitx, ploty, color='yellow')
plt.plot(right_fitx, ploty, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)

first frame

(720, 0)

Calculate the curve radius and offset to centor of road in unit of meters¶

def get_stats(ploty, left_fit, right_fit):
    # Define y-value where we want radius of curvature
    # I'll choose the maximum y-value, corresponding to the bottom of the image
    y_eval = np.max(ploty)
    
    # Define conversions in x and y from pixels space to meters
    ym_per_pix = 30/720 # meters per pixel in y dimension
    xm_per_pix = 3.7/700 # meters per pixel in x dimension

    leftx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
    rightx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
    
    # Fit new polynomials to x,y in world space
    left_fit_cr = np.polyfit(ploty*ym_per_pix, leftx*xm_per_pix, 2)
    right_fit_cr = np.polyfit(ploty*ym_per_pix, rightx*xm_per_pix, 2)
    # Calculate the new radii of curvature
    left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])
    right_curverad = ((1 + (2*right_fit_cr[0]*y_eval*ym_per_pix + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])
    # Now our radius of curvature is in meters
    line_pos_left = left_fit[0]*y_eval**2 + left_fit[1]*y_eval + left_fit[2]
    line_pos_right = right_fit[0]*y_eval**2 + right_fit[1]*y_eval + right_fit[2]
    offset = ((line_pos_left + line_pos_right) / 2 - 640) * xm_per_pix
    return left_curverad, right_curverad, offset, line_pos_left, line_pos_right

print(get_stats(ploty, left_fit, right_fit))

(1167.0040611385377, 475.50979799082, 0.13768365073003938, 367.21990145555054, 964.87661503689674)

Project the lane line area back to undistorted input image¶

def project_lane_lines(binary_warped, undist, left_fitx, right_fitx, ploty, MInv):
    # Create an image to draw the lines on
    warp_zero = np.zeros_like(binary_warped).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))

    # Recast the x and y points into usable format for cv2.fillPoly()
    pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
    pts = np.hstack((pts_left, pts_right))
    # Draw the lane onto the warped blank image
    cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))

    # Warp the blank back to original image space using inverse perspective matrix (Minv)
    newwarp = cv2.warpPerspective(color_warp, MInv, (image.shape[1], image.shape[0])) 
    # Combine the result with the original image

    result = cv2.addWeighted(undist, 1, newwarp, 0.3, 0)
    return result

result = project_lane_lines(binary_warped, undist, left_fitx, right_fitx, ploty, MInv)
left_curverad, right_curverad, offset, line_pos_left, line_pos_right = get_stats(ploty, left_fit, right_fit)
str_curverad = 'Radius of curvature = {0:.2f}m'.format((left_curverad + right_curverad) / 2)
str_offset = 'Vehicle is {} m {} from center'.format(round(offset,2), 'right' if offset > 0 else 'left')
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(result,str_curverad,(0,100), font, 2,(255,255,255),2,cv2.LINE_AA)
cv2.putText(result,str_offset,(0,200), font, 2,(255,255,255),2,cv2.LINE_AA)    
plt.figure(figsize=(24, 9))
plt.imshow(result)

<matplotlib.image.AxesImage at 0x11365c4e0>

def process_image(img_path, mtx, dist, M, MInv):
    image=plt.imread(img_path)
    undist = cv2.undistort(image, mtx, dist, None, mtx)
    b_color, b_combined = binarize(undist)
    imshape = b_combined.shape
    vertices = np.array([[(100, imshape[0]),(imshape[1]/2-10, imshape[0]/2+42),(imshape[1]/2+10, imshape[0]/2+42), (imshape[1], imshape[0])]], dtype=np.int32)
    roi = region_of_interest(b_combined, vertices)
    img_size = (imshape[1], imshape[0])
    # Warp the image using OpenCV warpPerspective()
    binary_warped = cv2.warpPerspective(roi, M, img_size, flags=cv2.INTER_LINEAR)
    out_img, left_fitx, right_fitx, ploty, left_fit, right_fit = fit_lane_line(binary_warped)
    result = project_lane_lines(binary_warped, undist, left_fitx, right_fitx, ploty, MInv)
    left_curverad, right_curverad, offset, line_pos_left, line_pos_right = get_stats(ploty, left_fit, right_fit)
    str_curverad = 'Radius of curvature = {0:.2f}m'.format((left_curverad + right_curverad) / 2)
    str_offset = 'Vehicle is {} m {} from center'.format(round(offset,2), 'right' if offset > 0 else 'left')
    font = cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(result,str_curverad,(0,100), font, 2,(255,255,255),2,cv2.LINE_AA)
    cv2.putText(result,str_offset,(0,200), font, 2,(255,255,255),2,cv2.LINE_AA)    
    plt.figure(figsize=(24, 9))
    plt.imshow(result)

process_image('test_images/test6.jpg', mtx, dist, M, MInv)

first frame

Video Pipeline and Sanity check of detected lane lines¶

# Define a class to receive the characteristics of each line detection
class Line():
    def __init__(self):
        # was the line detected in the last iteration?
        self.detected = False  
        # x values of the last n fits of the line
        self.recent_xfitted = [] 
        # average x values of the fitted line over the last n iterations
        self.bestx = None    
        # polynomial coefficients of the last n fits
        self.recent_fits = []
        #polynomial coefficients averaged over the last n iterations
        self.best_fit = None  
        #polynomial coefficients for the most recent fit
        self.current_fit = [np.array([False])]  
        #radius of curvature of the line in some units
        self.radius_of_curvature = None 
        #distance in meters of vehicle center from the line
        self.line_base_pos = None 
        #x values for detected line pixels
        self.allx = None  
        
    def update(self, pos, curverad, fit, fitx):
        self.detected = True
        self.radius_of_curvature = curverad
        self.current_fit = fit
        self.allx = fitx
        if len(self.recent_xfitted) > 5:
            self.recent_xfitted.pop(0)
        self.recent_xfitted.append(pos)
        self.bestx = sum(self.recent_xfitted) / len(self.recent_xfitted)
        if len(self.recent_fits) > 5:
            self.recent_fits.pop(0)
        self.recent_fits.append(fit)
        self.best_fit = [x/len(self.recent_fits) for x in [ sum(x) for x in zip(*self.recent_fits) ]]

def line_sanity(left, right, line_pos_left, line_pos_right, left_curverad, right_curverad, left_fit, right_fit, left_fitx, right_fitx):
    # left and right line has similar curvature
    if np.linalg.norm(left_fit - right_fit) / np.linalg.norm((left_fit+right_fit)/2) < 1.3:
        # left and right line are not too far away from center
        if 640 - line_pos_left < 500 and line_pos_right - 640 < 500:
            # left and right x position are not too far from previous detection
            if abs(left.bestx - line_pos_left) < 25 and abs(right.bestx - line_pos_right) < 25:
                # left and right coefficients are close to previous detection
                if np.linalg.norm(left_fit - left.best_fit) < 200 and np.linalg.norm(right_fit - right.best_fit) < 200:
                    # radius difference within 10x
                    if max(left_curverad,right_curverad) / min(left_curverad,right_curverad) < 10:
                        left.update(line_pos_left, left_curverad, left_fit, left_fitx)
                        right.update(line_pos_right, right_curverad, right_fit, right_fitx)
                        return True
                    else:
                        print(abs(left_curverad - right_curverad))
                        print('failed sanity: radius diff')
                else:
                    print(np.linalg.norm(left_fit - left.best_fit))
                    print(np.linalg.norm(right_fit - right.best_fit))
                    print('failed sanity: left and right line coefficient')
            else:
                print(abs(left.bestx - line_pos_left))
                print(abs(right.bestx - line_pos_right))
                print('failed sanity: line distance from previous detection')
        else:
            print('failed sanity: line distance from center')
    else:
        print(np.linalg.norm(left_fit - right_fit) / np.linalg.norm((left_fit+right_fit)/2))
        print('failed sanity: left and right line curvature')
    left.detected = False
    right.detected = False
    return False

def process_image_in_video(image):
    global mtx
    global dist
    global M
    global MInv
    global left_fit_prev
    global right_fit_prev
    global left
    global right
    global frame_count
    undist = cv2.undistort(image, mtx, dist, None, mtx)
    b_color, b_combined = binarize(undist)
    imshape = b_combined.shape
    vertices = np.array([[(100, imshape[0]),(imshape[1]/2-10, imshape[0]/2+42),(imshape[1]/2+10, imshape[0]/2+42), (imshape[1], imshape[0])]], dtype=np.int32)
    roi = region_of_interest(b_combined, vertices)
    img_size = (imshape[1], imshape[0])
    # Warp the image using OpenCV warpPerspective()
    binary_warped = cv2.warpPerspective(roi, M, img_size, flags=cv2.INTER_LINEAR)
    try:
        out_img, left_fitx, right_fitx, ploty, left_fit, right_fit = fit_lane_line(binary_warped, left_fit_prev, right_fit_prev)
    except:
        left_fitx = left.allx
        right_fitx = right.allx
        ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
        left_fit = left.current_fit
        right_fit = right.current_fit

    left_curverad, right_curverad, offset, line_pos_left, line_pos_right = get_stats(ploty, left_fit, right_fit)
    # sanity check, if detected, update and use the new line detected, otherwise use the previous line
    if 0 == frame_count:
        result = project_lane_lines(binary_warped, undist, left_fitx, right_fitx, ploty, MInv)
        left.update(line_pos_left, left_curverad, left_fit, left_fitx)
        right.update(line_pos_right, right_curverad, right_fit, right_fitx)
    elif line_sanity(left, right, line_pos_left, line_pos_right, left_curverad, right_curverad, left_fit, right_fit, left_fitx, right_fitx):
        result = project_lane_lines(binary_warped, undist, left_fitx, right_fitx, ploty, MInv)
        left_fit_prev = left_fit
        right_fit_prev = right_fit
    else:
        result = project_lane_lines(binary_warped, undist, left.allx, right.allx, ploty, MInv)
        left_curverad, right_curverad, offset, line_pos_left, line_pos_right = get_stats(ploty, left.current_fit, right.current_fit)

    str_curverad = 'Radius of curvature = {0:.2f}m'.format((left_curverad + right_curverad) / 2)
    str_offset = 'Vehicle is {} m {} from center'.format(round(abs(offset),2), 'right' if offset > 0 else 'left')
    font = cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(result,str_curverad,(0,100), font, 2,(255,255,255),2,cv2.LINE_AA)
    cv2.putText(result,str_offset,(0,200), font, 2,(255,255,255),2,cv2.LINE_AA) 
    
    frame_count += 1
    return result

# Import everything needed to edit/save/watch video clips
from moviepy.editor import VideoFileClip
out_dir='./'
output = out_dir+'processed_project_video.mp4'
left_fit_prev = right_fit_prev = None
frame_count = 0
left, right = Line(), Line()
clip = VideoFileClip("project_video.mp4")
out_clip = clip.fl_image(process_image_in_video) 
%time out_clip.write_videofile(output, audio=False)

first frame
[MoviePy] >>>> Building video ./processed_project_video.mp4
[MoviePy] Writing video ./processed_project_video.mp4

  0%|          | 1/1261 [00:00<06:13,  3.38it/s]

first frame
7328.90839543
failed sanity: radius diff

  0%|          | 2/1261 [00:00<06:05,  3.45it/s]

first frame

  0%|          | 3/1261 [00:00<05:59,  3.50it/s]

17378.5884457
failed sanity: radius diff

  1%|          | 13/1261 [00:03<05:36,  3.71it/s]

5101.08717263
failed sanity: radius diff

  1%|▏         | 16/1261 [00:04<05:26,  3.82it/s]

output = out_dir+'processed_challenge_video.mp4'
left_fit_prev = right_fit_prev = None
frame_count = 0
left, right = Line(), Line()
clip = VideoFileClip("challenge_video.mp4")
out_clip = clip.fl_image(process_image_in_video) 
out_clip.write_videofile(output, audio=False)

first frame
[MoviePy] >>>> Building video ./processed_challenge_video.mp4
[MoviePy] Writing video ./processed_challenge_video.mp4

  0%|          | 1/485 [00:00<02:21,  3.41it/s]

first frame

  1%|          | 6/485 [00:01<02:01,  3.94it/s]

27.7658878585
42.5283973016
failed sanity: line distance from previous detection

  1%|▏         | 7/485 [00:01<02:10,  3.68it/s]

24.9740281583
42.5283973016
failed sanity: line distance from previous detection

  2%|▏         | 8/485 [00:02<02:05,  3.81it/s]

17.4919023391
43.0283973016
failed sanity: line distance from previous detection

  2%|▏         | 10/485 [00:02<02:05,  3.78it/s]